
%%matlab初始化
clc;             %清除命令窗口的内容
clear all;       %清除工作空间的所有变量，函数，和MEX文件
close all;       %关闭所有的figure窗口

%方格数及障碍物比例的设定
n = 20;   % 产生一个n x n的方格，修改此值可以修改生成图片的方格数
obstaclepercent = 0.3;  % 这个变量代表生成的障碍物占总方格数的比例 ，如0.5 表示障碍物占总格数的50%

%方格以及障碍物的创建
[field, startposind, goalposind, costchart, fieldpointers] =initializeField(n,obstaclepercent); %随机生成包含障碍物，起始点，终止点等信息的矩阵

% 这个函数用来随机生成环境，障碍物，起点，终点
axishandle = createFigure(field,costchart,startposind,goalposind);    %将随机生成的方格及障碍物的数据生成图像

% 路径规划中用到的一些矩阵的初始化
setOpen = [startposind];                   %用来存放待选的子节点（也就是拓展出来的子节点）的矩阵，初始化为起始点的索引值
setOpenCosts = [0];                        %存放拓展出来的最优值距离起始点的代价，初始化为0（因为第一个点就是起始点，起始点距离）
setOpenHeuristics = [Inf];                 %存放待选的子节点距离终止点的代价
setClosed = []; setClosedCosts = [];       %在这个待选的矩阵里面找到最优的值，再存放到矩阵setClosed 中，初始化为空矩阵；
movementdirections = {'R','L','D','U'};    %移动方向,movementdirections中存放着四个移动方向的代号



%% 创建initializeField函数
function [field, startposind, goalposind, costchart, fieldpointers] = ...
  initializeField(n,obstaclepercent)
field = ones(n,n) + 1*rand(n,n); %生成一个n*n的单位矩阵+0到1范围内的一个随机数
field(ind2sub([n n],ceil(n^2.*rand(n*n*obstaclepercent,1)))) = Inf;%向上取整
% 一般来说，可以使用公式 r = a + (b-a).*rand(N,1) 生成区间 (a,b) 内的 N 个随机数。
% 其中rand(n* n* obstaclepercent,1)用来生成一个n* n*obstaclepercent行，1列的随机数向量，
% n * n *obstaclepercent也就是障碍物的数量，rand生成的是0到1范围内的小数，
% 将其乘以n的平方后，也就是n * n *obstaclepercent个0到n的平方范围内的数，这个数呢有可能是小数，
% 利用向上取整函数ceil将其变为整数。
% ind2sub函数用于把数组中元素索引值转换为该元素在数组中对应的下标，
% 这样field(ind2sub([n n],ceil(n^2.*rand(n*n*obstaclepercent,1))))就得到了障碍物在该矩阵下的下标，将矩阵中所有障碍物的位置赋为Inf

startposind = sub2ind([n,n],ceil(n.*rand),ceil(n.*rand));
goalposind = sub2ind([n,n],ceil(n.*rand),ceil(n.*rand));
field(startposind) = 0; field(goalposind) = 0; %把矩阵中起始点和终止点处的值设为0
% ind = sub2ind(sz,row,col) 针对大小为 sz 的矩阵返回由 row 和 col 指定的行列下标的对应线性索引 ind。
% 此处，sz 是包含两个元素的向量，其中 sz(1) 指定行数，sz(2) 指定列数。
%  ceil(n.*rand),ceil(n.*rand)用于随机生成一个位于nxn的矩阵内的一个下标，然后通过sub2ind函数，
%  将下标值转换为索引值，以上两行代码就得到了随机生成的起始点的索引值赋给变量startposind ，
%  终止点的索引值赋值给变量goalposind ，然后把矩阵中起始点和终止点处的值设为0

costchart = NaN*ones(n,n);%生成一个nxn的矩阵costchart，每个元素都设为NaN。就是矩阵初始NaN无效数据
costchart(startposind) = 0;%在矩阵costchart中将起始点位置处的值设为0

fieldpointers = cell(n,n);%生成元胞数组n*n
fieldpointers{startposind} = 'S';
fieldpointers{goalposind} = 'G'; %将元胞数组的起始点的位置处设为 'S'，终止点处设为'G'
fieldpointers(field==inf)={0};  %将障碍物处设为0
% 生成的field矩阵中，障碍物的位置处设为Inf，没有障碍物的位置处为0到1的随机数
% 生成元胞数组fieldpointers组，没有障碍物的地方的随机数设为0，起始点的位置处设为 'S'，终止点处设为'G'
% 生成一个costchart矩阵，每个元素都设为NaN，将起始点位置处的值设为0
end

%% 创建createFigure
function axishandle = createFigure(field,costchart,startposind,goalposind)
if isempty(gcbf)
%   figure('Position',[450 150 600 600], 'MenuBar','none');
    figure('Position',[450 150 600 600]);
    axes('position', [0.01 0.01 0.99 0.99]);
else
    gcf; cla;
end
% 其中gcbf作用是当前返回图像的句柄；isempty(gcbf)：假如gcbf为空的话，返回的值是1，假如gcbf为非空的话，返回的值是0；
% 接下来的语句figure(‘Position’,[450 50 700 700], ‘MenuBar’,‘none’);是对创建的figure图像进行设置，
% 指定为 [left bottom width height] 形式的向量。此区域不包括图窗边框、标题栏、菜单栏和工具栏。
% 设置其距离屏幕左侧的距离为450，距离屏幕下方的距离为50，长度和宽度都为700，'MenuBar','none'关闭图像的菜单栏；
% 接下来的语句 axes(‘position’, [0.01 0.01 0.99 0.99])是设置坐标轴的位置，左下角的坐标设为0.01,0.01
% 右上角的坐标设为0.99 0.99 （可以认为figure图的左下角坐标为0 0 ，右上角坐标为1 1 ）；
% gcf 作用是返回当前 Figure 对象的句柄值，然后利用cla语句来清除它

n = length(field);  %获取矩阵的长度，并赋值给变量n
field(field < Inf) = 0; %将fieid矩阵中的随机数（也就是没有障碍物的位置处）设为0

 pcolor(1:n+1,1:n+1,[field field(:,end); field(end,:) field(end,end)]);%多加了一个重复的（由n X n变为 n+1 X n+1 ）
%  函数中的1:n+1,1:n+1是用来描述矩阵的维度的，也就是这个矩阵是（n+1）X（n+1）的，
%  那么为什么要变成（n+1）X（n+1）而不是使用之前的n x n 的，
%  这是因为 pcolor函数是通过插值来实现的，插值后会缺少一行一列，
%  这样要想保持最后生成的方格数是nxn的就得先将其扩展成（n+1）X（n+1）
 
% % cmap = flipud(colormap('jet')); %colormap实际上是一个mx3的矩阵，每一行的3个值都为0-1之间数，分别代表颜色组成的rgb值
% % cmap(1,:) = zeros(3,1); cmap(end,:) = ones(3,1); %将矩阵cmap的第一行设为0 ，最后一行设为1
% % % cmap(1,:) = zeros; cmap(end,:) = ones;
% % colormap(flipud(cmap)); %进行颜色的倒转
% % colormap(flipud(jet))可以实现颜色的倒转，若colormap原来是Jet大值为红，小值为蓝色，
% % 则执行该语句后则把colormap按Jet格式倒转，即大值为蓝色，小值为红

 hold on;
 axishandle = pcolor([1:n+1],[1:n+1],[costchart costchart(:,end); costchart(end,:)  costchart(end,end)]);  %将矩阵costchart进行拓展，插值着色后赋给axishandle
 [goalposy,goalposx] = ind2sub([n,n],goalposind); %将终止点坐标的索引值转换成坐标值
 [startposy,startposx] = ind2sub([n,n],startposind); %将起始点点坐标的索引值转换成坐标值
 plot(goalposx+0.5,goalposy+0.5,'rs','MarkerSize',11,'LineWidth',5); %绘制出起点
 plot(startposx+0.5,startposy+0.5,'go','MarkerSize',10,'LineWidth',5); %绘制出终点,go:绿色的圆
 uicontrol('Style','pushbutton','String','RE-DO', 'FontSize',12, ...
     'Position', [1 1 60 40], 'Callback','astardemo');
% uicontrol 函数用来创建用户界面控件对象是在窗体上创建各种组件（比如、按钮、静态文本框、弹出式菜单等），并指定这些组件的回调函数。
% Style用来选择要生成的uicontrol 对象的类型，其后的pushbutton表示的生成的对象类型是按钮键，
% String这个属性声明了显示在生成对象上的标签字符串，也就是紧跟其后的RE-DO，
% FontSize用来设置字体的大小，
% Position用来设置生成对象的位置，
% Callback是主回调函数，将回调属性值指定为函数句柄、元胞数组或字符向量的详细信息

end

%% [costs,heuristics,posinds] = findFValue(setOpen(ii),setOpenCosts(ii),  field,goalposind,'euclidean');
% 这个函数的输入参数中setOpen(ii)表示拓展点，
% setOpenCosts(ii)表示当前拓展点的代价，
% field是之前生成的环境信息，
% goalposind是终止点的索引值，
% euclidean 是拓展的方法
% 这个函数的作用就是把输入的点作为父节点，然后进行拓展找到子节点，并且找到子节点的代价，并且把子节点距离终点的代价找到，
% 函数的输出量中costs表示拓展的子节点到起始点的代价，heuristics表示拓展出来的点到终止点的距离大约是多少，posinds表示拓展出来的子节点
% 注意无论是坐标值还是索引值他表示的都是方格的左下角的那个点，而不是整个方格，也可以理解为我们用方格的左下角的点来表示该方格

    function [cost,heuristic,posinds] =  findFValue(posind,costsofar,field,goalind,heuristicmethod)
    n = length(field);  % 获取矩阵的长度
    [currentpos(1) currentpos(2)] = ind2sub([n n],posind);   %将要进行拓展的点（也就是父节点）的索引值拓展成坐标值
    [goalpos(1) goalpos(2)] = ind2sub([n n],goalind);        %将终止点的索引值拓展成坐标值
    cost = Inf*ones(4,1); heuristic = Inf*ones(4,1); pos = ones(4,2);  
%    将矩阵cost和heuristic初始化为4x1的无穷大值的矩阵，pos初始化为4x2的值为1的矩阵，
%    为什么是4x1的呢，因为我们拓展的时候采用的是向上下左右四个方向进行拓展，也就是最多可以找到四个子节点，
%    所以代价矩阵是4x1的，为啥是4x2的呢，因为pos中存放找到的子节点的坐标值，一个子节点需要两个位置，分别存放横纵坐标

    %方向一
    newx = currentpos(2) - 1; newy = currentpos(1);
    if newx > 0
      pos(1,:) = [newy newx]; %只要拓展出来的这个点没有超出边界就把这个点的坐标值放到pos中
      switch lower(heuristicmethod)
        case 'euclidean'
          heuristic(1) = abs(goalpos(2)-newx) + abs(goalpos(1)-newy);
        case 'taxicab'
          heuristic(1) = abs(goalpos(2)-newx) + abs(goalpos(1)-newy);
      end
      cost(1) = costsofar + field(newy,newx);   %计算出拓展出来的点距离起始点的代价或者说距离
    end

%     因为我们要从要进行拓展的点（也就是父节点）向上下左右四个方向进行尝试，看一下其能否被拓展，成为子节点，
%     也就是在我们得到的目前要进行拓展的点的坐标上对其横坐标+1、-1，纵坐标不变；纵坐标+1、-1，横坐标不变。
%     得到四个方向上相邻的点的横纵坐标newx、newy。 比如：newx = curren tpos(2) - 1; newy = currentpos(1)。
%     接下来 if newx > 0 是用来判断拓展出来的点是否超越了边界，因为我们拓展出来的点一定要在我们之前创建的nxn的环境中，
%     在向下或者向左拓展时要判断其坐标是否大于0，在向上或者向右拓展时要判断是否<=n;
%     switch-end 以上代码是用来选择计算终止点距离拓展出来的点代价（或者说距离）的方法，并且计算出终止点距离拓展出来的点的代价（或者说距离）




    %方向二
    newx = currentpos(2) + 1; newy = currentpos(1);
    if newx <= n
      pos(2,:) = [newy newx];
      switch lower(heuristicmethod)
        case 'euclidean'
          heuristic(2) = abs(goalpos(2)-newx) + abs(goalpos(1)-newy);
        case 'taxicab'
          heuristic(2) = abs(goalpos(2)-newx) + abs(goalpos(1)-newy);
      end
      cost(2) = costsofar + field(newy,newx);
    end

    %方向三
    newx = currentpos(2); newy = currentpos(1)-1;
    if newy > 0
      pos(3,:) = [newy newx];
      switch lower(heuristicmethod)
        case 'euclidean'
          heuristic(3) = abs(goalpos(2)-newx) + abs(goalpos(1)-newy);
        case 'taxicab'
          heuristic(3) = abs(goalpos(2)-newx) + abs(goalpos(1)-newy);
      end
      cost(3) = costsofar + field(newy,newx);
    end

    %方向四
    newx = currentpos(2); newy = currentpos(1)+1;
    if newy <= n
      pos(4,:) = [newy newx];
      switch lower(heuristicmethod)
        case 'euclidean'
          heuristic(4) = abs(goalpos(2)-newx) + abs(goalpos(1)-newy);
        case 'taxicab'
          heuristic(4) = abs(goalpos(2)-newx) + abs(goalpos(1)-newy);
      end
      cost(4) = costsofar + field(newy,newx);
    end
    
 
    posinds = sub2ind([n n],pos(:,1),pos(:,2));
    end

%% 这个while循环是本程序的核心，利用循环进行迭代来寻找终止点
while ~max(ismember(setOpen,goalposind)) && ~isempty(setOpen)
   % ismember(setOpen,goalposind)这个函数就是把矩阵setOpen里的点在矩阵goalposind中进行查找，找到了就返回1，找不到返回0
   % setOpen是从起始点开始进行拓展的，若是拓展到了终止点，也就是setOpen中包含终止点，
   % 也就是ismember(setOpen,goalposind)的返回值中其中一个元素为1，那么只要我找到了（也就是setopen拓展到了）终止点，
   % max(ismember(setOpen,goal posind))的值就是1，没找到就是0，
   % 我们需要在他没找到的时候进行循环所以需要取反一下 ~max(ismember(setOpen,goalposind)) ，
   % 此外除了找到终点停止循环，在无路可走时，比如起始点的周围都是障碍物时，也需要停止循环，
   % 利用isempty(setOpen) 如果 setOpen为空，isempty(setOpen) 返回逻辑值 1，否则返回逻辑值 0 ，
   % 如果setOpen为空表示没有可以拓展的点，也就是无路可走，此时需要跳出循环，因此也需要取反一下： ~ isempty(setOpen) 
   % 这两个条件要同时满足，所以将其用&&连起来，也就构成了判断循环的条件 while ~max(ismember(setOpen,goalposind)) && ~isempty(setOpen)
    [temp, ii] = min(setOpenCosts + setOpenHeuristics);     %寻找拓展出来的最小值 
    
    %这个函数的作用就是把输入的点作为父节点，然后进行拓展找到子节点，并且找到子节点的代价，并且把子节点距离终点的代价找到
    [costs,heuristics,posinds] = findFValue(setOpen(ii),setOpenCosts(ii), field,goalposind,'euclidean');
 
  setClosed = [setClosed; setOpen(ii)];     % 将找出来的拓展出来的点中代价最小的那个点串到矩阵setClosed 中 
  setClosedCosts = [setClosedCosts; setOpenCosts(ii)];    % 将拓展出来的点中代价最小的那个点的代价串到矩阵setClosedCosts 中
  
  % 从setOpen中删除刚才放到矩阵setClosed中的那个点
  %如果这个点位于矩阵的内部
  if (ii > 1 && ii < length(setOpen))
    setOpen = [setOpen(1:ii-1); setOpen(ii+1:end)];
    setOpenCosts = [setOpenCosts(1:ii-1); setOpenCosts(ii+1:end)];
    setOpenHeuristics = [setOpenHeuristics(1:ii-1); setOpenHeuristics(ii+1:end)];
    
  %如果这个点位于矩阵第一行
  elseif (ii == 1)
    setOpen = setOpen(2:end);
    setOpenCosts = setOpenCosts(2:end);
    setOpenHeuristics = setOpenHeuristics(2:end);
    
  %如果这个点位于矩阵的最后一行
  else
    setOpen = setOpen(1:end-1);
    setOpenCosts = setOpenCosts(1:end-1);
    setOpenHeuristics = setOpenHeuristics(1:end-1);
  end
  
%%












